# Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import threading
import signal
import time
import subprocess
import select
import os
from packagetool import PackageTool

class CustomNodeManager:
    @staticmethod
    def GetInstance():
        if not hasattr(CustomNodeManager, "_instance"):
            CustomNodeManager._instance = CustomNodeManager()
        return CustomNodeManager._instance

    def __init__(self):
        self.nodes = {}
    
    def operate(self,name,token,operate,fn,code,cb):
        if operate == "start":
            if token in self.nodes:
                print("node already exists", token)
                return
            else:
                self.nodes[token] = {"token": token, "fn": fn, "code": code, "cb": cb, "exit": False}
                print("start node", token)
                threading.Thread(target=self.run_node, args=(self.nodes[token],)).start()
        elif operate == "stop":
            if token not in self.nodes:
                os.system("ps -ef | grep %s | grep -v grep | awk '{print $2}' | xargs kill -9"%fn)
                subprocess.run(["rosnode", "kill", "/"+name])
                subprocess.run(["rosnode", "cleanup"], input="y\n", text=True)
                print("node not exists", token)
                return
            else:
                self.nodes[token]["cb"] = cb
                self.nodes[token]["exit"] = True
        elif operate == "reload":
            self.operate(name,token,"stop",fn,code,cb)
            for i in range(20):#等待2秒退出成功
                if token not in self.nodes:
                    break
                time.sleep(0.1)
            self.operate(name,token,"start",fn,code,cb)
                
    def result(self,cb,token,status,log):
        pt_ret = PackageTool()
        pt_ret.put_string("custom_node_operate_ret")
        pt_ret.put_string(token)
        pt_ret.put_string(status)
        pt_ret.put_string(log)
        cb(pt_ret)
        
    def run_node(self,node):
        with open(node["fn"], "w") as f:
            f.write(node["code"])
        proc = subprocess.Popen(["python", "-u", node["fn"]],stdin=subprocess.PIPE,stdout=subprocess.PIPE,stderr=subprocess.PIPE,text=True)
        self.result(node["cb"],node["token"],"running","running")
        while not node["exit"]:
            if proc.poll() is not None:
                break
            rlist, _, _ = select.select([proc.stdout, proc.stderr], [], [], 0.1)
            if proc.stdout in rlist:
                line = proc.stdout.readline()
                self.result(node["cb"],node["token"],"out",line.strip())
            if proc.stderr in rlist:
                line = proc.stderr.readline()
                self.result(node["cb"],node["token"],"err",line.strip())
        print("node exiting", node["token"])
        self.result(node["cb"],node["token"],"stopped","stopped")
        if proc.poll() is None:
            proc.send_signal(signal.SIGINT)# 1. 先尝试正常终止（发送 SIGINT，模拟 Ctrl+C）
            time.sleep(1)  # 等待节点正常关闭

            if proc.poll() is None:# 2. 如果仍然运行，强制终止
                proc.terminate()# 发送 SIGTERM
                time.sleep(0.5)
                if proc.poll() is None:
                    proc.kill()# 发送 SIGKILL（强制终止）

        # 3. 清理 ROS 节点注册信息
        subprocess.run(["rosnode", "cleanup"], input="y\n", text=True)
        
        del self.nodes[node["token"]]
        print("node exited", node["token"])